#include "Components/clock.h"
#include "Components/motor.h"
#include "Components/oled_hardware_i2c.h"
#include "Components/uart.h"
#include "ti_msp_dl_config.h"
#include <ti/driverlib/m0p/dl_interrupt.h>


/**
 * 解析格式为 0xaa + data len + data + 0xff 的数据包
 * @param packet 输入数据包指针
 * @param packet_len 数据包长度
 * @param data_len 输出参数：数据部分的长度
 * @return 数据部分的起始地址，若解析失败则返回NULL
 */
uint8_t *parse_packet(uint8_t *buffer, size_t buffer_len, size_t *data_len)
{
  if (buffer_len < 3)
  {
    return NULL; // 缓冲区长度不足，无法包含完整数据包
  }

  // 搜索包头 0xaa
  uint8_t *ptr = buffer;
  uint8_t *end = buffer + buffer_len;

  while (ptr < end - 2)
  { // 确保剩余数据至少能容纳包头+长度+包尾
    if (*ptr == 0xaa)
    {
      // 找到包头，验证数据包
      size_t len = ptr[1];
      size_t expected_packet_len = len + 3;

      if (ptr + expected_packet_len <= end)
      { // 数据包完整存在于缓冲区中
        if (ptr[expected_packet_len - 1] == 0xff)
        { // 验证包尾
          *data_len = len;
          return ptr + 2; // 返回数据起始地址
        }
      }
    }
    ptr++; // 继续搜索下一个可能的包头
  }

  return NULL; // 未找到有效数据包
}

uint8_t getNumFromStr(const char *str, uint8_t len)
{
  uint8_t ret = 0;
  int pow = 1;
  for (int i = len - 1; i >= 0; i--)
  {
    ret += (str[i] - '0') * pow;
    pow *= 10;
  }
  return ret;
}

//+00,000,0
int parse_position_string(const char *input, int input_len, int *x, uint8_t *theta, char *position)
{
  if (input_len != 9)
    return -1;

  if (input[0] == '+')
    *x = getNumFromStr(input + 1, 2);
  else
    *x = -getNumFromStr(input + 1, 2);

  *theta = getNumFromStr(input + 4, 3);
  *position = input[8];

  return 0;
}

int main(void)
{
  SYSCFG_DL_init();
  SysTick_Init();

  uart_init();
  OLED_Init();
  motor_init();


  OLED_ShowString(0, 0, (uint8_t *)"TI MSPM0 TEST", 16);

  DL_GPIO_clearPins(LEDs_PORT, LEDs_LED_PIN);
  uart0_send_string("Init sucessfully!\n");

  while (1)
  {
    uint8_t *serial_data = NULL;
    uint16_t serial_data_len = getCurrentUart1Data(&serial_data);

    if (serial_data && (serial_data_len != 0))
    {
      setCurrentUART1DataLock(true);

      size_t rec_len = 0;
      uint8_t *rec = parse_packet(serial_data, serial_data_len, &rec_len);

      // uart0_send_string("Rec serial data : ");
      // uart0_send_str_with_len((char *)serial_data, serial_data_len);
      // uart0_send_string("\n");

      int x;
      uint8_t theta;
      char position;
      parse_position_string((const char *)rec, rec_len, &x, &theta, &position);

      // char buffer[32] = {0};
      // sprintf(buffer, "(%d,%u,%c)", x, theta, position);

      // uart0_Log_str("uart parse", buffer);

      setCurrentUART1DataLock(false);
    }

  }
}
